/*
 * Copyright (c) 2019, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2019-08-26     sogwms       The first version
 */

#include "controller.h"

// single intput and single output system

controller_t controller_create(unsigned long size, unsigned short sample_time)
{
    // Malloc memory and initialize PID
    controller_t new_controller = (controller_t)malloc(size);
    if(new_controller == NULL)
    {
        aos_cli_printf("Failed to malloc memory for new controller\n");
        return NULL;
    }

    new_controller->sample_time = sample_time;
    new_controller->enable = 0;

    return new_controller;
}

signed long controller_update(controller_t controller, float current_point)
{
    CHECK_RET_WITH_RET(controller, -1);

    if (controller->enable)
    {
        return controller->update(controller, current_point);
    }
    else
    {
        controller->output = controller->target;
    }
    
    return 0;
}

signed long controller_destroy(controller_t controller)
{
    CHECK_RET_WITH_RET(controller, -1);
    
    aos_cli_printf("Free controller");

    return controller->destroy(controller);
}

signed long controller_enable(controller_t controller)
{
    CHECK_RET_WITH_RET(controller, -1);

    aos_cli_printf("Enabling controller");
    controller->enable = 1;

    return 0;
}

signed long controller_disable(controller_t controller)
{
    CHECK_RET_WITH_RET(controller, -1);

    aos_cli_printf("Disabling controller");
    controller->enable = 0;

    return 0;
}

signed long controller_reset(controller_t controller)
{
    CHECK_RET_WITH_RET(controller, -1);

    return controller->reset(controller);
}

signed long controller_set_target(controller_t controller, signed short target)
{
    CHECK_RET_WITH_RET(controller, -1);

    controller->target = target;
    return 0;
}

signed long controller_set_sample_time(controller_t controller, unsigned short sample_time)
{
    CHECK_RET_WITH_RET(controller, -1);

    controller->sample_time = sample_time;
    return 0;
}

signed long controller_set_param(controller_t controller, controller_param_t param)
{
    CHECK_RET_WITH_RET(controller, -1);

    return controller->set_param(controller, param);
}

signed long controller_get_param(controller_t controller, controller_param_t param)
{
    CHECK_RET_WITH_RET(controller, -1);

    return controller->get_param(controller, param);
}

unsigned int aos_tick_from_millisecond(signed int ms)
{
    unsigned int tick;

    if (ms < 0)
    {
        tick = (unsigned int)-1;
    }
    else
    {
        tick = AOS_TICK_PER_SECOND * (ms / 1000);
        tick += (AOS_TICK_PER_SECOND * (ms % 1000) + 999) / 1000;
    }

    /* return the calculated tick */
    return tick;
}
